#include "Octree.h"

Octree::Octree(const Vec3 &origin, const Vec3 &radius) :
    _origin(origin), _radius(radius) {
}

Octree::Octree(Octree &&other) :
    _origin(other._origin), _radius(other._radius) {
    for (int i = 0; i < 8; ++i) {
        _data = other._data;
        _children[i] = other._children[i];
        other._children[i] = nullptr;
    }
}

Octree::~Octree() {
    if (_children[0] == nullptr) return;
    for (int i = 0; i < 8; ++i)
        delete _children[i];
}

Octree Octree::buildOctree(const Vec3 &origin, const Vec3 &radius, const TingSTL::array<Vec3> &points) {
    auto oct = Octree(origin, radius);
    for (Vec3 &data : points) {
        oct.insert(&data);
    }
    return oct;
}

bool Octree::isOutBox(const Vec3 &bmin, const Vec3 &bmax) const {
    Vec3 max = _origin + _radius;
    Vec3 min = _origin - _radius;

    if (max.x < bmin.x || max.y < bmin.y || max.z < bmin.z) return true;
    if (min.x > bmax.x || min.y > bmax.y || min.z > bmax.z) return true;

    return false;
}

void Octree::insert(Vec3 *data) {
    if (isLeafNode()) {
        // 叶子节点  如果空节点插入即可， 如有有数据，需要如果是新数据插入，数据已经存在了，ret
        if (_data == nullptr) {
            TING_DEBUG("insert Data x:%f,y: %f,z:%f", data->x, data->y, data->z);
            _data = data;
        } else {
            // if(*_data == *data) return;
            auto oldData = _data;
            _data = nullptr;
            buildChildren();
            auto bPos1 = getBoxPos(*oldData);
            auto bPos2 = getBoxPos(*data);
            _children[(int)bPos1]->insert(oldData);
            _children[(int)bPos2]->insert(data);
        }
    } else {
        // 非叶子节点，在对应子节点插入数据
        auto bPos = getBoxPos(*data);
        _children[(int)bPos]->insert(data);
    }
}

TingSTL::array<Vec3 *> Octree::getPointsInsideBox(const Vec3 &bmin, const Vec3 &bmax) const {
    TingSTL::array<Vec3 *> ret;
    _getPointsInsideBox(bmin, bmax, ret);
    return ret;
}

EBoxPos Octree::getBoxPos(const Vec3 &pos) const {
    int ret = 0;
    if (pos.x >= _origin.x) ret |= BOX_POS_X_MASK;
    if (pos.y >= _origin.y) ret |= BOX_POS_Y_MASK;
    if (pos.z >= _origin.z) ret |= BOX_POS_Z_MASK;
    return (EBoxPos)ret;
}

void Octree::buildChildren() {
    Vec3 newRadius = _radius * 0.5f;
    for (int i = 0; i < 8; ++i) {
        Vec3 newOrigin = _origin;
        newOrigin.x += _radius.x * (i & BOX_POS_X_MASK ? 0.5f : -0.5f);
        newOrigin.y += _radius.y * (i & BOX_POS_Y_MASK ? 0.5f : -0.5f);
        newOrigin.z += _radius.z * (i & BOX_POS_Z_MASK ? 0.5f : -0.5f);
        _children[i] = new Octree(newOrigin, newRadius);
    }
}

void Octree::_getPointsInsideBox(const Vec3 &bmin, const Vec3 &bmax, TingSTL::array<Vec3 *> &points) const {
    if (isLeafNode()) {
        if (_data != nullptr) {
            if (_data->isInBox(bmin, bmax))
                points.emplace_back(_data);
        }
    } else {
        for (int i = 0; i < 8; ++i) {
            if (_children[i]->isOutBox(bmin, bmax)) return;
            _children[i]->_getPointsInsideBox(bmin, bmax, points);
        }
    }
}